/**
 Copyright will be fixed soon.
 Author: Hiroshi Ota.
 $Date: $
 $Revision: $
*/

#include "BFDriveControl.h"
#include "IBFBehaviorControl.h"

BFState::DriveDir BFDriveControl::sdMap[4] = {
	BFState::DD_OFF,
	BFState::DD_FORWARD,
	BFState::DD_BACKWARD,
	BFState::DD_BRAKE
};


BFDriveControl::BFDriveControl(IBFBehaviorControl* bc)
: IBFArgorithm(), context(bc)
{
}

BFDriveControl::~BFDriveControl()
{
}

void BFDriveControl::updateDrive(PWPathFinderRobot::MotorType motorType, BFState::DriveDir dir, unsigned char level)
{
	PWPathFinderRobot* robot = context->getPathFinderRobot();
	if (dir==BFState::DD_OFF) {
		robot->motorOff(motorType);
//		motor->off();
	} else if (dir==BFState::DD_BRAKE) {
		robot->motorBrake(motorType);
//		motor->brake();
	} else {
		if (dir== BFState::DD_FORWARD) {
			robot->motorForward(motorType);
		} else if (dir==BFState::DD_BACKWARD) {
			robot->motorReverse(motorType);
		}
		robot->motorSpeed(motorType, level);
//		motor->direction((MotorDirection)dir);
//		motor->speed(level);
	}
}

int BFDriveControl::execute(unsigned int loopTick, BFState *state)
{
	updateDrive(PWPathFinderRobot::MOTOR_TYPE_DRIVE, state->currentDriveDir,state->currentDriveLevel);
	updateDrive(PWPathFinderRobot::MOTOR_TYPE_STEERING, sdMap[state->currentStairingDir],state->currentStairingLevel);

	return 0;
}
